#include "OdomReading.h"

OdomReading::OdomReading(nav_msgs::Odometry msg){
	this->x = msg.pose.pose.position.x;
	this->y = msg.pose.pose.position.y;
	this->theta = get_yaw(msg.pose.pose.orientation);
}

OdomReading::OdomReading(double x,double y,double theta){
	this->x = x;
	this->y = y;
	this->theta = theta;
}

double OdomReading::calculateDistanceTo(OdomReading &current){
	double dx = this->x - current.x;
	double dy = this->y - current.y;
	double dist = std::sqrt(dx*dx+dy*dy);
	return dist;
}

double OdomReading::calculateRotation(OdomReading &current){
	return (this->theta - current.theta);
}

OdomReading::~OdomReading() {
	// TODO Auto-generated destructor stub
}
